#include <linux/sched.h>
#include <linux/kernel.h>
#include <linux/fdreg.h>
#include <asm/system.h>
#include <asm/io.h>

#define MAJOR_NR 2
#include "blk.h"

#define MAX_REPLIES 7
static unsigned char reply_buffer[MAX_REPLIES];

extern void floppy_interrupt(void);

static void output_byte(char byte)
{
    int counter;
    unsigned char status;

    for(counter = 0; counter < 10000; counter++) {
        status = inb_p(FD_STATUS) & (STATUS_READY | STATUS_DIR);
        if (status == STATUS_READY) {
            outb(byte, FD_DATA);
            return;
        }
    }
    printk("Unable to send byte to FDC\n\r");
}

static int result(void)
{
    int i = 0, counter, status;

    for (counter = 0; counter < 10000; counter++) {
        status = inb_p(FD_STATUS) & (STATUS_DIR | STATUS_READY | STATUS_BUSY);
        if (status == STATUS_READY)
            return i;
        if (status == (STATUS_DIR | STATUS_READY | STATUS_BUSY)) {
            if (i >= MAX_REPLIES)
                break;
            reply_buffer[i++] = inb_p(FD_DATA);
        }
    }
    printk("Getstatus times out\n\r");
    return -1;
}

static void seek_interrupt(void)
{
    output_byte(FD_SENSEI);
    result();
    printk("seek interrupt\n\r");
}

static void transfer(void)
{
    do_floppy = seek_interrupt;
    output_byte(FD_RECALIBRATE);
    output_byte(0);
}

void unexpected_floppy_interrupt(void)
{
    output_byte(FD_SENSEI);
    result();
}

void do_fd_request(void)
{
    floppy_on(0);   // 开启电动机
    transfer();
    floppy_off(0);
}

void floppy_init(void)
{
    set_trap_gate(0x26, &floppy_interrupt);
    outb(inb_p(0x21) & ~0x40, 0x21);
}
